// This is the main DLL file.

#include "stdafx.h"

#include "CameraCal.h"

#include "Ravl/SArray1dIter.hh"

namespace CameraCalN
{

	bool CalObservation2dArrayC::NormalScale(Matrix3dC& T) const
	{
		UIntT count(0);
		SArray1dIterC<CalObservation2dC> i(*this);
		Vector2dC offset(0,0);
		RealT scale(0.0);

		// sum points
		for (i.First(); i; i++)
		{
			if ((*i).visible)
			{
				offset += (*i).u;
				count++;
			}
		}

		// must have some visible points
		if (count == 0)
		{
			T = Matrix3dC(1,0,0,0,1,0,0,0,1);
			return false;
		}

		// compute centroid offset
		offset /= count;

		// do offset, compute scale
		for (i.First(); i; i++)
		{
			if ((*i).visible)
			{
				RealT du = (*i).u[0] - offset[0];
				RealT dv = (*i).u[1] - offset[1];
				scale += sqrt(du*du + dv*dv);
			}
		}

		// actually find mean scale, divided by root(2)
		scale /= (count*sqrt(RealT(2)));

		// get the scale from normalised co-ords to current co-ords
		T = Matrix3dC( scale, 0, offset[0],
			0, scale, offset[1],
			0,     0,    1      );

		return true;
	}

	void CalObservation2dArrayC::TransformHomogeneous(Matrix3dC& T)
	{
		for (SArray1dIterC<CalObservation2dC> i(*this); i; i++)
		{
			Vector2dC& ui = (*i).u;
			RealT u = T[0][0]*ui[0] + T[0][1]*ui[1] + T[0][2];
			RealT v = T[1][0]*ui[0] + T[1][1]*ui[1] + T[1][2];
			RealT w = T[2][0]*ui[0] + T[2][1]*ui[1] + T[2][2];
			ui[0] = u / w;
			ui[1] = v / w;
		}
	}

}
